package edu.wpi.first.wpilibj.swarm;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStationEnhancedIO;
import edu.wpi.first.wpilibj.DriverStationEnhancedIO.EnhancedIOException;

public class DSInputs {
	DriverStationEnhancedIO io;
	
	public DSInputs() {
		io = DriverStation.getInstance().getEnhancedIO();
	}
	
	void init() {
		try {
			io.setDigitalConfig(1, DriverStationEnhancedIO.tDigitalConfig.kInputPullUp);
			io.setDigitalConfig(2, DriverStationEnhancedIO.tDigitalConfig.kInputPullUp);
			io.setDigitalConfig(3, DriverStationEnhancedIO.tDigitalConfig.kInputPullUp);
			io.setDigitalConfig(4, DriverStationEnhancedIO.tDigitalConfig.kInputPullUp);
			io.setDigitalConfig(5, DriverStationEnhancedIO.tDigitalConfig.kInputPullUp);
			io.setDigitalConfig(6, DriverStationEnhancedIO.tDigitalConfig.kInputPullUp);
			io.setDigitalConfig(7, DriverStationEnhancedIO.tDigitalConfig.kInputPullUp);
			io.setDigitalConfig(8, DriverStationEnhancedIO.tDigitalConfig.kInputPullUp);
			io.setDigitalConfig(9, DriverStationEnhancedIO.tDigitalConfig.kInputPullUp);
			io.setDigitalConfig(10, DriverStationEnhancedIO.tDigitalConfig.kInputPullUp);
			io.setDigitalConfig(11, DriverStationEnhancedIO.tDigitalConfig.kInputPullUp);
			io.setDigitalConfig(12, DriverStationEnhancedIO.tDigitalConfig.kInputPullUp);
			io.setDigitalConfig(13, DriverStationEnhancedIO.tDigitalConfig.kInputPullUp);
			io.setDigitalConfig(14, DriverStationEnhancedIO.tDigitalConfig.kInputPullUp);
			io.setDigitalConfig(15, DriverStationEnhancedIO.tDigitalConfig.kInputPullUp);
			io.setDigitalConfig(16, DriverStationEnhancedIO.tDigitalConfig.kInputPullUp);
		} catch (EnhancedIOException ex) {}
	}
	
	int getAutonomousMode() {
		try {
			if (!io.getDigital(13)) return 1;
			if (!io.getDigital(14)) return 2;
			if (!io.getDigital(15)) return 3;
			if (!io.getDigital(16)) return 4;
		} catch (Exception e) {}
		return 0;
	}

	boolean getReset() {
		try {
			return !io.getDigital(3);
		} catch (EnhancedIOException ex) {}
		return false;
	}

	boolean getBalance() {
		try {
			return !io.getDigital(9);
		} catch (EnhancedIOException ex) {}
		return false;
	}

	boolean getField() {
		try {
			return io.getDigital(1);
		} catch (EnhancedIOException ex) {}
		return true;
	}

	boolean getArmUp() {
		try {
			return !io.getDigital(5);
		} catch (EnhancedIOException ex) {}
		return false;
	}

	boolean getArmReady() {
		try {
			return !io.getDigital(8);
		} catch (EnhancedIOException ex) {}
		return false;
	}

	boolean getArmLiftReady() {
		try {
			return !io.getDigital(10);
		} catch (EnhancedIOException ex) {}
		return false;
	}

	boolean getArmDown() {
		try {
			return !io.getDigital(11);
		} catch (EnhancedIOException ex) {}
		return false;
	}

	boolean getBrushForward() {
		try {
			return !io.getDigital(4);
		} catch (EnhancedIOException ex) {}
		return false;
	}

	boolean getBrushReverse() {
		try {
			return !io.getDigital(2);
		} catch (EnhancedIOException ex) {}
		return false;
	}

	boolean getKick() {
		try {
			return !io.getDigital(12);
		} catch (EnhancedIOException ex) {}
		return false;
	}

	double getDistance() {
		try {
			return io.getAnalogInRatio(2);
		} catch (EnhancedIOException ex) {}
		return 0.5;
	}
	
	double getArmPos() {
		try {
			return io.getAnalogInRatio(1);
		} catch (EnhancedIOException ex) {}
		return 0.0;
	}

	boolean getHighSpeed() {
		try {
			return io.getDigital(7);
		} catch (EnhancedIOException ex) {}
		return true;
	}

	boolean getCompressor() {
		try {
			return io.getDigital(6);
		} catch (EnhancedIOException ex) {}
		return false;
	}
}
